/*
 * mt9v03x.c
 *
 *  Created on: 2025年3月12日
 *      Author: shuai
 */

#include "mt9v03x.h"
#include "math.h"
#include "circle.h"
int16  first_end;
int16  l_start;
int16  r_start;
int16  l_effect_num;
int16  r_effect_num;
int16 search_l_start[MT9V03X_H], search_l_end[MT9V03X_H];   //左边界开始结束数组
int16 search_r_start[MT9V03X_H], search_r_end[MT9V03X_H];   //右边界开始结束数组


int l_knee_point1 = 0; // 定义变量l_knee_point1，用于存储第一个左拐点的位置，初始化为0
int l_knee_point2 = 0; // 定义变量l_knee_point2，用于存储第二个左拐点的位置，初始化为0

int r_knee_point1 = 0; // 定义变量r_knee_point1，用于存储第一个右拐点的位置，初始化为0
int r_knee_point2 = 0; // 定义变量r_knee_point2，用于存储第二个右拐点的位置，初始化为0

int16 l_border_bl[MT9V03X_H], r_border_bl[MT9V03X_H];        //左右边界补线数组数 
int8  l_effect_flag[MT9V03X_H], r_effect_flag[MT9V03X_H];   //左右边界有效标志位

int16 l_border[MT9V03X_H], r_border[MT9V03X_H];   //左右边界数组数 /*/值为x轴的数据 范围为1-188
int16 obstacle_rxl=0;
int16 obstacle_lxl=0;
int16 current_white=0;                            //当前白点
int16 image_position[MT9V03X_H];                  //搜索的中线与摄像头中线的偏差值

#define THRESHOLD 14     // 有效白点判断阈值//15


/**
 * @brief 更新当前的灰度阈值
 * 
 * @param data 图像数组指针(学长这里用的是最下面一行)
 * @param num 采样个数
 */
void search_white(uint8 *data, uint16 num) 
{
    int16 white_lost = 0; // // 初始化白点丢失数量为0
    int16 histogram[256]={0}; // 初始化灰度直方图数组，用于统计每个灰度级别的像素数量
    int16 white_start=0, white_end=0; // 白点开始的灰度和结束的灰度
    // 初始化白点开始和结束的灰度值为0
    int16 temp_value=0, temp_value1=0;

    // 统计最近一行(其实就是最下面的一行)的灰度直方图
    for(int i = 0; i < num; i++)
    {
        histogram[*data]++; // 根据当前像素的灰度值增加对应灰度级别的计数
        data++; // 移动到下一个像素
    }

    // 从最高灰度级别开始遍历直方图，寻找白点的范围
    for(int i = 255; i >= 0; i--)
    {
        if(!white_start) // 如果没有找到白点开始的灰度
        {
            if(histogram[i] > 5) // 如果当前灰度级别的像素数量大于5，则认为是白点的开始
                white_start = i;
        }
        else
        {
            if(!histogram[i]) // 如果当前灰度级别的像素数量为0
            {
                white_end = i + white_lost; // 设置白点结束的灰度值
                if(histogram[i] < 5) white_lost++; // 如果之前连续的非零灰度级别像素数量小于5，则增加丢失数量
            }
            else
            {
                white_lost = 0; // 如果遇到非零灰度级别的像素，则重置丢失数量
            }
        }
        if(white_lost >= 5) break; // 如果丢失数量超过5，则提前结束循环
    }

    // 计算白点的中间灰度值
    temp_value = white_end + (white_start - white_end) / 5;

    // 如果当前没有记录的白点值
    if(!current_white)
    {
        current_white = temp_value; // 将计算出的白点中间灰度值设置为当前白点值
    }
    else
    {
        // 校验白点合法性
        temp_value1 = calc_diff(temp_value, current_white); // 计算新计算出的白点与当前白点的差异
        if(temp_value1 > -THRESHOLD / 2) // 如果差异大于负阈值的一半
        {
            current_white = current_white + (int16)limit(temp_value - current_white, 10); // 更新当前白点值，但更新量不超过10
        }
    }
}



// 定义二维数组white_length，用于存储每列的白色区域的起始和结束行号
int16 white_length[2][(SEARCH_MAX - SEARCH_MIN + 1) / SEARCH_WH_DEN];

// 定义二维数组white_length_max，用于存储最长的白色列的起始和结束行号
int16 white_length_max[2];


// 以上一帧的最长白列为“种子列”，优先在其±窗口内选取，避免跨车道
static int16 g_longest_col_seed = MT9V03X_W / 2;
#define LONGEST_COL_PREF_WIN 26   // 种子优先窗口半宽，可按实际调 20~30

void search_longest_white_col(void)
{
    int16 i, j;
    int16 temp_value;
    const int16 count = (SEARCH_MAX - SEARCH_MIN + 1) / SEARCH_WH_DEN;
    white_length_max[0] = MT9V03X_H - 1;
    white_length_max[1] = (int16)func_limit_ab(g_longest_col_seed, SEARCH_MIN, SEARCH_MAX);
    // 记录是否在“优先小窗”内找到候选
    int found_in_pref = 0;
    // ---------- Pass 1：在种子±窗口内优先搜索 ----------
    for (i = 0; i < count; i++)
    {
        white_length[1][i] = (int16)(i * SEARCH_WH_DEN + SEARCH_MIN);
        // 仅保留小窗内的列
        if (func_abs(white_length[1][i] - g_longest_col_seed) > LONGEST_COL_PREF_WIN)
            continue;
        // 自底向上找该列的白段结束行
        for (j = MT9V03X_H - 1; j > MT9V03X_H - 6; j--)
        {
            temp_value = calc_diff(current_white, mt9v03x_image[j][white_length[1][i]]);
            white_length[0][i] = j;
            if (temp_value > THRESHOLD) break;
        }
        if (j == MT9V03X_H - 6)
        {
            for (; j >= 0; j--)
            {
                temp_value = calc_diff(mt9v03x_image[j + JUMP_NUM][white_length[1][i]],
                                       mt9v03x_image[j][white_length[1][i]]);
                white_length[0][i] = j;
                if (temp_value > THRESHOLD) break;
            }
        }
        // 选取“最高的白段结束行”
        if (white_length[0][i] < white_length_max[0])
        {
            white_length_max[0] = white_length[0][i];
            white_length_max[1] = white_length[1][i];
            found_in_pref = 1;
        }
    }
    // ---------- Pass 2：小窗内没找到则退回原全域搜索（保持原逻辑） ----------
    if (!found_in_pref)
    {
        white_length_max[0] = MT9V03X_H - 1; // 重新初始化
        for (i = 0; i < count; i++)
        {
            white_length[1][i] = (int16)(i * SEARCH_WH_DEN + SEARCH_MIN);
            for (j = MT9V03X_H - 1; j > MT9V03X_H - 6; j--)
            {
                temp_value = calc_diff(current_white, mt9v03x_image[j][white_length[1][i]]);
                white_length[0][i] = j;
                if (temp_value > THRESHOLD) break;
            }
            if (j == MT9V03X_H - 6)
            {
                for (; j >= 0; j--)
                {
                    temp_value = calc_diff(mt9v03x_image[j + JUMP_NUM][white_length[1][i]],
                                           mt9v03x_image[j][white_length[1][i]]);
                    white_length[0][i] = j;
                    if (temp_value > THRESHOLD) break;
                }
            }
            if (white_length[0][i] < white_length_max[0])
            {
                white_length_max[0] = white_length[0][i];
                white_length_max[1] = white_length[1][i];
            }
        }
    }
    // ---------- 更新“种子列”（用于下一帧的优先小窗） ----------
    g_longest_col_seed = white_length_max[1];
}

/**
 * @brief 搜索并更新左右边界
 * @note 更新l_border[MT9V03X_H]与r_border[MT9V03X_H];
 *
 */
void search_border(void) // 定义一个函数，用于搜索边界
{
    int16 temp_value = 0; // 定义两个16位整型变量，用于临时存储计算结果

    first_end = white_length_max[0] + 5; // 计算first_end的值，与白色长度最大值有关
    if (first_end > MT9V03X_H - 1) first_end = MT9V03X_H - 1; // 如果first_end超过图像的高度，则限制其值为最底行

    l_start = 0; // 初始化左边界起始行为0
    r_start = 0; // 初始化右边界起始行为0
    l_effect_num = 0; // 初始化左边界有效行数为0
    r_effect_num = 0; // 初始化右边界有效行数为0

    // 从图像底部向上遍历到first_end行
    for (int i = MT9V03X_H - 1; i > first_end; i--)
    {
        //最长白列为中心行，从此为左右搜线的起始列
        search_l_start[i] = white_length_max[1]; 
        search_r_start[i] = white_length_max[1]; 

        //设置当前行的左边界为1，右搜索边界为180-2
        search_l_end[i] = SEARCH_MIN; 
        search_r_end[i] = SEARCH_MAX; 

        // 清除边界有效标志位
        l_effect_flag[i] = 0; // 当前行的左边界尚未找到有效值

        //循环:从起始列search_l_start[i]向左(j--)搜索
        for (int j = search_l_start[i]; j >= search_l_end[i]; j--)
        {   
            // 核心:计算当前点i与其右侧j+JUMP_NUM点的差异，即梯度
            int16_t gv = calc_diff(mt9v03x_image[i][j + JUMP_NUM], mt9v03x_image[i][j]); 
            //无论是否找到，都先暂存当前位置
            l_border[i] = j; // 更新当前行的左边界列坐标
            l_border_bl[i]=j;// 补线数组存储

            //判断梯度是否够大
            if (gv > THRESHOLD)
            {
                l_effect_flag[i] = 1; // 标记为有效边界点
                if (!l_start) l_start = i; // 如果是第一次找到，记录行号
                l_effect_num++; // 有效点计数加一
                break; // 找到左边界后退出循环
            }
        }

        // 清除边界有效标志位
        r_effect_flag[i] = 0; // 当前行的右边界尚未找到有效值

        // 搜索右边
        for (int j = search_r_start[i]; j <= search_r_end[i]; j++)
        {
            int16_t gv = calc_diff(mt9v03x_image[i][j - JUMP_NUM], mt9v03x_image[i][j]); // 计算灰度梯度

            r_border[i] = j; // 更新当前行的右边界位置

            r_border_bl[i]=j; //补线数组存储

            if (gv > THRESHOLD) // 如果差异大于某个阈值
            {
                r_effect_flag[i] = 1; // 标记当前行的右边界为有效
                if (!r_start) r_start = i; // 如果尚未找到右边界起始行，则设置当前行为起始行
                r_effect_num++; // 右边界有效行数加一

                break; // 找到右边界后退出循环
            }
    
        }
    }

    /*远方估计填充,我疑心这就是扫到其他赛道问题的根源*/
    // 从近景搜索的终点(first_end)开始，一直处理到图像最顶端(0)
    for (int i = first_end; i >= 0; i--)
    {
        // 选取一个可靠“参考点”
        // 这个点位于近景搜索结果的上边缘，是已知信息中最靠近未知区域的，因此最具有代表性
        temp_value = first_end + 3;
        if (temp_value > (MT9V03X_H - 1)) // 防止越界
        {
            temp_value = MT9V03X_H - 1;
        }

        //参考行中线与宽度
        const int mid_ref = (l_border[temp_value] + r_border[temp_value]) / 2;
        int width_ref=r_border[temp_value]-l_border[temp_value];
        //宽度保护:避免异常过小或者过大
        const int width_min = 10; //最小宽度，请按赛道实际微调
        const int width_max = SEARCH_MAX - SEARCH_MIN;
        if(width_ref<width_min) width_ref=width_min;
        if(width_ref>width_max) width_ref=width_max;

        int l=mid_ref-(width_ref/2);
        int r=l+width_ref;

        l=func_limit_ab(l,SEARCH_MIN,SEARCH_MAX);
        r=func_limit_ab(r,SEARCH_MIN,SEARCH_MAX);

        if(r-l<width_min) 
        {
            int expand=(width_min-(r-l)+1)/2;
            l=func_limit_ab(l-expand,SEARCH_MIN,SEARCH_MAX);
            r=func_limit_ab(r+expand,SEARCH_MIN,SEARCH_MAX);
        }

        l_border[i]=(int16_t)l;
        r_border[i] =(int16_t)r;
    }
}

#define l_KNEE_SLOPE_THRESHOLD -10//寻找第二个拐点时，判断进入急弯的列坐标差值阈值
#define l_KNEE_FLAT_THRESHOLD 5   //判断弯道变缓(趋于直线)的列坐标差值绝对值阈值
#define l_KNEE2_CONFIRM_COUNT 2 //需要连续多少个点满足变缓条件才确认出弯

typedef enum
{
    searching_knee1,       // 阶段一:正在寻找第一个拐点(入弯点)
    searching_knee2_start, // 阶段二:已经找到第一个，正在寻找第二个拐点的急弯起始点
    search_knee2_confirm,  // 阶段三:已经找到急弯点,正在确认出弯的平缓区
} KneePointSearchState;



/**
 * @brief 搜索左转弯时的两个拐点
 * @note 更新l_knee_point1第一个左拐点行坐标与l_knee_point2第二个右拐点行坐标
 * @param jump 计算斜率(列坐标之差)的步长
 */
void search_l_knee_point(int jump)
{
    int a=0, b=0; // 存储前后差分值
    int start=0, end=0; // 搜素的起始与结束行号

    // 初始化状态为寻找第一个拐点
    KneePointSearchState state = searching_knee1;

    uint8_t confirm_counter = 0;

    // 1.确定搜索范围，从有效边界的起始点附近开始，向上搜索
    start = l_start - jump; // 从左边界起始行开始，减去跳跃步长
    end = first_end + jump; // 到first_end行结束，加上跳跃步长

    //约束范围，确保数组访问不会越界
    start = limit_ab(start, 8 + jump, MT9V03X_H - jump);
    end = limit_ab(end, 8 + jump, MT9V03X_H - jump);

    //初始化拐点值为0，表示尚未找到
    l_knee_point1 = 0;
    l_knee_point2 = 0;
    //从start行开始，向上(i--)遍历
    for (int i = start; i >= end; i--)
    {
        //4.计算当前点i前后“斜率”的离散表示
        //a:当前点i与它下方点(i+jump)的列坐标差值
        a = l_border[i] - l_border[i + jump];//下
        //b:当前点i与它上方点(i-jump)的列坐标差值
        b = l_border[i - jump]-l_border[i];//上

        switch(state)
        {
            case searching_knee1:
                //左边界拐点列坐标比上下都大，向内凸
                if(a > 0 && b <0 && i - jump != first_end)
                {
                    l_knee_point1 = i; // 将当前行标记为第一个左拐点位置
                    state = searching_knee2_start; // 转换状态，开始寻找第二个拐点的急弯起始点
                }
            break;
            
            //寻找弯道最陡峭的部分
            case searching_knee2_start:
                if(b<l_KNEE_SLOPE_THRESHOLD)
                {
                    state=search_knee2_confirm;
                    confirm_counter=0;
                }
            break;

            case search_knee2_confirm:
                if(myabs(b)<l_KNEE_FLAT_THRESHOLD)
                {
                    confirm_counter++;//满足变缓条件，计数值加一

                    if(confirm_counter>=l_KNEE2_CONFIRM_COUNT)
                    {
                        l_knee_point2=i;//连续满足条件，确认找到第二个拐点
                        goto search_done;//所有任务完成，直接跳出循环
                    }
                }

                else
                {   //如果不满足，则重置计数器，防止误判
                    confirm_counter=0;
                }
            break;

            default:
            break;
        }
    }

    search_done:
    ; // 标签，用于跳出多层循环
}

#define R_KNEE_SLOPE_THRESHOLD 10 // 寻找第二个拐点时，判断进入急弯的列坐标差值阈值
#define R_KNEE_FLAT_THRESHOLD 5    // 判断弯道变缓(趋于直线)的列坐标差值绝对值阈值
#define R_KNEE2_CONFIRM_COUNT 2    // 需要连续多少个点满足变缓条件才确认出弯

/**
 * @brief 搜索右转弯时的有边界的两个拐点
 * @note 更新r_knee_point1第一个左拐点行坐标与r_knee_point2第二个右拐点行坐标
 * @param jump 计算斜率(列坐标之差)的步长
 */
void search_r_knee_point(int jump) // 定义一个函数，用于搜索右拐点，jump参数表示每次跳跃的步长
{
    int a, b; // 用于存储边界位置差值的变量
    int start, end; // 定义搜索的起始和结束位置
    uint8_t confirm_counter = 0;
    // 初始化状态为寻找第一个拐点
    KneePointSearchState state = searching_knee1;

    // 计算搜索的起始和结束位置
    start = r_start - jump; // 从右边界起始行开始，减去跳跃步长
    end = first_end + jump; // 到first_end行结束，加上跳跃步长

    // 对起始和结束位置进行限幅处理，确保它们不会超出图像的范围
    start = limit_ab(start, 8 + jump, MT9V03X_H - jump);
    end = limit_ab(end, 8 + jump, MT9V03X_H - jump);

    // 初始化第一个右拐点位置为0
    r_knee_point1 = 0;
    r_knee_point2 = 0;

    for (int i = start; i >= end;i--)
    {
        a=r_border[i]-r_border[i+jump];//下
        b=r_border[i-jump]-r_border[i];//上

        switch(state)
        {
            case searching_knee1:
                if(a<0&&b>0&&i-jump!=first_end)
                {
                    r_knee_point1=i;
                    state=searching_knee2_start;
                }
            break;

            case searching_knee2_start:
                if(b>R_KNEE_SLOPE_THRESHOLD)
                {
                    state=search_knee2_confirm;
                    confirm_counter=0;
                }
            break;

            case search_knee2_confirm:
                if(myabs(b)<R_KNEE_FLAT_THRESHOLD)
                {
                    confirm_counter++;//满足变缓条件，计数值加一

                    if(confirm_counter>=R_KNEE2_CONFIRM_COUNT)
                    {
                        r_knee_point2=i;//连续满足条件，确认找到第二个拐点
                        goto search_done;//所有任务完成，直接跳出循环
                    }
                }

                else
                {   //如果不满足，则重置计数器，防止误判
                    confirm_counter=0;
                }
            break;

            default:
            break;
        }
    }
    search_done:;
}


/**
 * @brief 计算最小二乘法直线拟合参数
 * 
 * @param start 起始行坐标
 * @param end 结尾行坐标
 * @param border 边界数组
 * @param effect 有效点数组
 * @param a 斜率
 * @param b 截距
 */
void least_square_method(int16 start, int16 end, int16 *border, int8 *effect, float *a, float *b)
{
    // 基本健壮性校验（避免空指针、返回值未定义）
    if (!border || !effect || !a || !b)
    {
        return;
    }
    // 初始化输出，保证在任何早退路径下都有定义
    *a = 0.0f;
    *b = 0.0f;
    // 处理start/end顺序，确保从较大索引向较小索引遍历逻辑成立
    // 与原逻辑兼容：原实现仅处理 start>=end 的倒序循环

    const float EPS = 1e-3f;

    int16 s = start;
    int16 e = end;
    if (s < e)
    {
        int16 tmp = s;
        s = e;
        e = tmp;
    }
    // Welford在线算法变量
    // n: 有效点计数；mx: x均值；my: y均值；Sxx: x关于均值的平方和；Sxy: x与y关于均值的协方差累加项
    uint16 n = 0U;
    float mx = 0.0f;
    float my = 0.0f;
    float Sxx = 0.0f;
    float Sxy = 0.0f;
    // 单次扫描累积
    for (int16 i = s; i >= e; i--)
    {
        if (effect[i])
        {
            const float x = (float)border[i];
            const float y = (float)i;
            n++;
            const float inv_n = 1.0f / (float)n;
            // 先更新x均值并累加Sxx、Sxy（Welford算法）
            const float dx = x - mx;
            mx += dx * inv_n;
            // y均值同样在线更新
            const float dy = y - my;
            my += dy * inv_n;
            // 注意：此处使用更新后的均值计算的“第二项”，保持数值稳定
            Sxx += dx * (x - mx);
            Sxy += dx * (y - my);
        }
    }
    // 无有效点，直接返回默认输出
    if (n == 0U)
    {
        // 计算窗口内（不看effect）的x均值作为竖线位置，更稳妥，不会落在0列
        int32_t acc = 0;
        int32_t cnt = 0;
        for (int16 i = s; i >= e; i--)
        {
            int16 xi = border[i];
            // 简单合法性约束，防止越界值参与（可按需保留/删除）
            if (xi >= SEARCH_MIN && xi <= SEARCH_MAX)
            {
                acc += xi;
                cnt++;
            }
        }
        float mean_x;
        if (cnt > 0)
        {
            mean_x = (float)acc / (float)cnt;
        }
        else
        {
            // 若全部不可用，退回到图像中线附近，避免0列
            mean_x = (float)((SEARCH_MIN + SEARCH_MAX) / 2);
        }
        // 限幅，避免异常值
        int16 ax = func_limit_ab((int16)lroundf(mean_x), SEARCH_MIN, SEARCH_MAX); // 需要 math.h
        *a = (float)ax;                                                           // 垂直线 x = a
        *b = 0.0f;                                                                // 斜率为0，fill_line会按竖线路径处理
        return;                                                                   // MOD: 新增兜底返回
    }
    // 计算方差与协方差（使用总体方差/协方差定义，保持与原始实现一致的尺度）
    const float var_x = (n > 0U) ? (Sxx / (float)n) : 0.0f;
    const float cov_xy = (n > 0U) ? (Sxy / (float)n) : 0.0f;
    // x方差为0 -> 垂直线：返回b=0，a为x的均值（与原先语义一致）
    // 使用严格等值判断以保持与原代码逻辑一致（仅在确实所有x相同的情况下进入）
    if (var_x < EPS)
    {
        int16 ax = func_limit_ab((int16)lroundf(mx), SEARCH_MIN, SEARCH_MAX); // MOD: 新增限幅
        *a = (float)ax;                                                       // 垂直线 x = a
        *b = 0.0f;
        return;
    }
    // 斜率与截距
    *b = cov_xy / var_x;
    *a = my - (*b) * mx;
}

void fill_line(int16 start, int16 end, int16 *border, int8 *effect, float a, float b,int8 type)
{
    //补线
    //如果是十字
    if(!type)
    {
        for(int i=end; i<=start; i++)
        {   
            if(fabsf(b)<1e-3)
            {
                border[i] = limit_ab((int16)roundf(a), SEARCH_MIN, SEARCH_MAX);
                continue;
            }
            border[i] = (int16)((i-a)/b + 0.5);
            border[i] = func_limit_ab(border[i], SEARCH_MIN, SEARCH_MAX);
        }
    }
    else
    {
        for(int i=end; i<start; i++)
        {
            if (fabsf(b) < 1e-3)
            {
                if (!effect[i])
                {
                    border[i] = func_limit_ab((int16)roundf(a), SEARCH_MIN, SEARCH_MAX);
                }
                continue;
            }
            if (!effect[i])
            {
                border[i] = (int16)((i - a) / b + 0.5f);
                border[i] = func_limit_ab(border[i], SEARCH_MIN, SEARCH_MAX);
            }
        }
    }
}

// --- 单拐点检测与处理参数 ---
// 触发单拐点修正的最小赛道宽度（像素）
#define CROSS_FILTER_MIN_WIDTH 40
// 用于拟合拐点周围边界的窗口大小（行数）
#define KNEE_FIT_WINDOW_SIZE 20
// 左拐点有效斜率范围（乘以100后的整数值）
#define L_KNEE_SLOPE_ABOVE_MIN (-60) // 拐点上方（远端）斜率下限
#define L_KNEE_SLOPE_ABOVE_MAX 0     // 拐点上方（远端）斜率上限
#define L_KNEE_SLOPE_BELOW_MIN 0     // 拐点下方（近端）斜率下限
#define L_KNEE_SLOPE_BELOW_MAX 100   // 拐点下方（近端）斜率上限
// 右拐点有效斜率范围
#define R_KNEE_SLOPE_ABOVE_MIN 0
#define R_KNEE_SLOPE_ABOVE_MAX 60
#define R_KNEE_SLOPE_BELOW_MIN (-100)
#define R_KNEE_SLOPE_BELOW_MAX 0
// --- 边界丢失与修复参数 ---
// 基于一侧边界推算另一侧丢失边界的基础偏移量
#define LOST_BORDER_BASE_OFFSET 38
// 触发近端边界补线的最小有效点数
#define LOST_LINE_MIN_EFFECTIVE_POINTS 20
// 近端边界补线时，用于拟合的窗口起始与结束偏移（相对于l_start/r_start）
#define LOST_LINE_FIT_START_OFFSET 5
#define LOST_LINE_FIT_END_OFFSET 20
// 在完全丢失近端边界时，重新确认边界起点的连续有效点计数
#define LOST_LINE_CONFIRM_COUNT 2


/**
 * @brief 单拐点补线函数
 * 
 * @param knee_point 拐点
 * @param border_self 左拐点
 * @param border_other 有拐点
 * @param effect_self 左拐点有效标志位数组
 * @param effect_other 有拐点有效标志位数组
 * @param effect_num_self 左拐点有效点个数
 * @param effect_num_other 有拐点有效点个数
 * @param is_left 测算左拐点还是右拐点
 */
static void handle_single_knee_point(int knee_point,int16 *restrict border_self, int16 *restrict border_other,int8 *restrict effect_self,int8 *restrict effect_other,int16 *effect_num_self,int16 *effect_num_other,int is_left)
{
    if(border_other[knee_point]-border_self[knee_point]<=CROSS_FILTER_MIN_WIDTH)
    {
        return;
    }

    float a = 0;//真正意义上的截距
    float b = 0;//真正意义上的斜率
    const int h_max=MT9V03X_H-1;
    //拟合拐点下方(近端)的斜率
    int start_hi=knee_point+KNEE_FIT_WINDOW_SIZE;
    if(start_hi>h_max)start_hi=h_max;
    least_square_method(start_hi,knee_point,border_self,effect_self,&a,&b);
    const int16 slope_below = (int16)(b * 100.0f);

    //拟合拐点上方(远端的斜率)
    int start_lo = knee_point - KNEE_FIT_WINDOW_SIZE;
    if(start_lo<0)start_lo=0;
    least_square_method(knee_point,start_lo,border_self,effect_self,&a,&b);
    const int16 slope_above = (int16)(b * 100.0f);

    //检查斜率是否符合预设的拐点模型
    int is_valid_slope=0;

    if(is_left)
    {
        if(slope_above>L_KNEE_SLOPE_ABOVE_MIN&&slope_above<L_KNEE_SLOPE_ABOVE_MAX&&slope_below>L_KNEE_SLOPE_BELOW_MIN&&slope_below<L_KNEE_SLOPE_BELOW_MAX)
        {
            is_valid_slope=1;
        }
    }

    else//is_right
    {
        if(slope_above > R_KNEE_SLOPE_ABOVE_MIN && slope_above < R_KNEE_SLOPE_ABOVE_MAX &&
            slope_below > R_KNEE_SLOPE_BELOW_MIN && slope_below < R_KNEE_SLOPE_BELOW_MAX)
        {
            is_valid_slope = 1;
        }
    }

    if(is_valid_slope)
    {
        fill_line(knee_point, 0, border_self, effect_self, a, b, 0);
        for (int i = knee_point; i >= 0;i--)
        {   //由于摄像头噪声、光照变化、地面反光等因素，边界检测时可能误把非赛道区域的高梯度点当作边界，导致边界点偏移。
            //如果某一行左边界检测到的点实际在右边界右侧，就会出现交叉。
            if (border_self[i] > border_other[i]) 
            {
                border_self[i] = border_other[i];
                if(effect_other[i])
                {
                    effect_other[i] = 0;
                    (*effect_num_other)--;
                }
            }
        }
    }

}

/**
 * @brief 十字填充线函数
 * 
 */
void cross_filter(void)
{
    int16 *restrict lb= l_border;
    int16 *restrict rb= r_border;
    int8 *restrict le= l_effect_flag;
    int8 *restrict re= r_effect_flag;
    const int h_max=MT9V03X_H-1;
    float a = 0;
    float b = 0;

    //第一部分,单拐点处理
    if(l_knee_point1&&!l_knee_point2)
    {
        handle_single_knee_point(l_knee_point1,lb,rb,le,re,&l_effect_num,&r_effect_num,1);
    }

    if(r_knee_point1&&!r_knee_point2)
    {
        handle_single_knee_point(r_knee_point1,rb,lb,re,le,&r_effect_num,&l_effect_num,0);
    }

    //第二部分,双拐点处理
    //场景:通常是完整的弯道或者S形弯，直接在两个拐点之间线性补线

    if(l_knee_point1&&l_knee_point2)
    {
        const int k1 = l_knee_point1;
        const int k2 = l_knee_point2;
        const int dx = (int)lb[k2] - (int)lb[k1];
        if (dx != 0)
        {
            b = (float)(k2 - k1) / (float)dx;
            a = (float)k2 - (float)lb[k2] * b;
        }
        else
        {
            b = 0.0f;
            a = (float)lb[k1];
        }
        fill_line(k1, k2, lb, le, a, b,0);
    }

    if (r_knee_point1 && r_knee_point2)
    {
        const int k1 = r_knee_point1, k2 = r_knee_point2;
        const int dx = (int)rb[k2] - (int)rb[k1];
        if (dx != 0)
        {
            b = (float)(k2 - k1) / (float)dx;
            a = (float)k2 - (float)rb[k2] * b;
        }
        else
        {
            b = 0.0f;
            a = (float)rb[k1];
        }
        fill_line(k1, k2, rb, re, a, b, 0);
    }

    //第三部分:单边丢失修复
    //场景:一侧边界完全丢失，但另一侧有清晰的双拐点，此时根据经验公式推算丢失测
    if(r_knee_point1&&r_knee_point2&&!l_knee_point1&&!l_knee_point2)
    {
        for (int i = r_knee_point2; i <= r_knee_point1;i++)
        {
            int v=(int)rb[i]-LOST_BORDER_BASE_OFFSET;
            lb[i] = (int16)(v > 0 ? v : 0);
        }
    }

    //第四部分:近端边界丢失修复
    //场景；赛道近处边界因阴影或反光而断裂，需要向上拟合以求补全
    if (l_start != h_max && l_effect_num > LOST_LINE_MIN_EFFECTIVE_POINTS)
    {
        least_square_method(l_start - LOST_LINE_FIT_START_OFFSET, l_start - LOST_LINE_FIT_END_OFFSET, lb, le, &a, &b);
        fill_line(h_max, l_start, lb, le, a, b,1);
    }
    if (r_start != h_max && r_effect_num > LOST_LINE_MIN_EFFECTIVE_POINTS)
    {
        least_square_method(r_start - LOST_LINE_FIT_START_OFFSET, r_start - LOST_LINE_FIT_END_OFFSET, rb, re, &a, &b);
        fill_line(h_max, r_start, rb, re, a, b,1);
    }
    // 注意：原代码中对 l_start/r_start == h_max 的处理逻辑较为复杂且可能与其他部分重叠，
    // 在此重构中，我们保留了更直接有效的近端补线逻辑。如果需要完全复现原逻辑，
    // 可将那部分代码也抽象成一个独立的辅助函数。
}


//扫描的行号(行坐标)
#define FINISH_LINE_SCAN_ROW_FAR 40 //远景扫描行
#define FINISH_LINE_SCAN_ROW_MID 60          // 中景扫描行
#define FINISH_LINE_SCAN_ROW_NEAR 80         // 近景扫描行

#define BORDER_REF_ROW_FAR 15          // 远景扫描使用的边界参考行
#define BORDER_REF_ROW_MID 25         // 中景扫描使用的边界参考行
#define BORDER_REF_ROW_NEAR 35         // 近景扫描使用的边界参考行

//触发终点线判定的最小跳变次数
#define MIN_TRANSITIONS_TO_TRIGGER 3

//终点标志位
uint8_t zhongdian_flag = 0;

/**
 * @brief 识别终点线函数
 * @note 更新zhongdian_flag终点标志位
 */
void check_finish_line(void)
{
    uint16_t transitions_far = 0;
    uint16_t transitions_mid = 0;
    uint16_t transitions_near = 0;
    int16_t diff_current = 0;
    int16_t diff_next = 0;

    //扫描远景行
    //注意:扫描第40行,但使用第15行的边界作为扫描范围

    const int16_t start_col_far=l_border[BORDER_REF_ROW_FAR];
    const int16_t end_col_far = r_border[BORDER_REF_ROW_FAR] - 2;

    for(int i=start_col_far; i<=end_col_far; i++)
    {
        diff_current=calc_diff(current_white,mt9v03x_image[FINISH_LINE_SCAN_ROW_FAR][i]);
        diff_next=calc_diff(current_white,mt9v03x_image[FINISH_LINE_SCAN_ROW_FAR][i+1]);
        //检查一个像素点是“黑”(差异大)而下一个是“白”(差异小)的特征跳变
        if(diff_current>THRESHOLD&&diff_next<(THRESHOLD/2))
        {
            transitions_far++;
        }
    }

    // --- 2. 扫描中景行 ---
    // 注意：扫描第60行，但使用第25行的边界作为扫描范围
    const int16_t start_col_mid = l_border[BORDER_REF_ROW_MID];
    const int16_t end_col_mid = r_border[BORDER_REF_ROW_MID] - 3;
    for (int i = start_col_mid; i < end_col_mid; i++)
    {
        diff_current = calc_diff(current_white, mt9v03x_image[FINISH_LINE_SCAN_ROW_MID][i]);
        diff_next = calc_diff(current_white, mt9v03x_image[FINISH_LINE_SCAN_ROW_MID][i + 1]);
        if (diff_current > THRESHOLD && diff_next < (THRESHOLD / 2))
        {
            transitions_mid++;
        }
    }
    // --- 3. 扫描近景行 ---
    // 注意：扫描第80行，但使用第35行的边界作为扫描范围
    const int16_t start_col_near = l_border[BORDER_REF_ROW_NEAR];
    const int16_t end_col_near = r_border[BORDER_REF_ROW_NEAR] - 3;
    for (int i = start_col_near; i < end_col_near; i++)
    {
        diff_current = calc_diff(current_white, mt9v03x_image[FINISH_LINE_SCAN_ROW_NEAR][i]);
        diff_next = calc_diff(current_white, mt9v03x_image[FINISH_LINE_SCAN_ROW_NEAR][i + 1]);
        if (diff_current > THRESHOLD && diff_next < (THRESHOLD / 2))
        {
            transitions_near++;
        }
    }
    // --- 4. 综合判断 ---
    // 如果任何一个扫描行的特征跳变次数超过阈值，则判定为终点线
    if (transitions_far  > MIN_TRANSITIONS_TO_TRIGGER ||
        transitions_mid  > MIN_TRANSITIONS_TO_TRIGGER ||
        transitions_near > MIN_TRANSITIONS_TO_TRIGGER)
    {
        zhongdian_flag = 1;
    }
}






uint8_t cross_flag = 0;
int16_t cross_distance = 0;

void search_line(void)///搜线
{
    uint8 i;
    //搜索最合适的白点
    search_white(&mt9v03x_image[MT9V03X_H-1][1],SEARCH_MAX-SEARCH_MIN+1);

    //从底部网上搜素最长白列
    search_longest_white_col();

    //在最长白列左右使用边沿跟踪算法搜索边界
    search_border();

    search_l_knee_point(2);     //搜索左拐点
    search_r_knee_point(2);     //搜索右拐点
    
    if (r_knee_point1 != 0 && r_knee_point2 != 0 && l_knee_point1 != 0 && l_knee_point2 != 0)
    {
        cross_flag = 1;
    }

    //circle(); // 环岛
    if ((circle_left == 0 && left_flag == 0) && (right_flag == 0 && circle_right == 0))
    {
        cross_filter(); // 十字
    }

    if(ting_cnt>1000)   //发车计时，防止发车时误认终点
    {
        check_finish_line();
    }

//    if(onece_flag==0){ obstacle();}

    //出界保护程序
    if(mt9v03x_image[MT9V03X_H-1][MT9V03X_W/2-20] < (current_white/2) && mt9v03x_image[MT9V03X_H-1][MT9V03X_W/2+20] < (current_white/2))
    {
        stop_flag = 1;//保护标志
        
    }
    
    if(zhongdian_flag==1)
    {
        
        stop_flag=1;
    }
    
     //计算每行理论中线跟实际中线的偏差
    for(i=0; i<MT9V03X_H; i++)
    {
        image_position[i] = (l_border[i] + r_border[i]-(MT9V03X_W))/2;//被除数越大中心越靠右
    }
}

int16 position=0, position_old=0;

void calc_corner(void)//计算偏差
{
    float posi = 0;

    for(int i=(MT9V03X_H-1); i>=45; i--)
    {
       posi += (float)image_position[i]*((float)0.5+((float)0.45*(100-i)/(119-0)));
    }

    position_old = position;                     //倒数第二次的position

    position = (int16)(posi/10);                 //倒数第一次position

    position=(int16)limit_ab(position,-350,350);

//    if(right_flag == 1)
//        position=limit_ab(position,-20,20);

//    if((circle_right==2||circle_right==3)&&position>95)
//           position=95;

//    if((circle_left==2||circle_left==3)&&position<-100)
//        position=-100;

//    if(zhongdian_flag==1)
//        position=limit_ab(position,-10,10);


}

uint8 onece_flag = 0;
int16 width1 =0;
int16 width2 =0;
int8  flag_obstacle = 0;
int16 obstacle_distance = 0;    //障碍物距离
int16 temp_ob = 0;
int8 ob_lose_l= 0;
int8 ob_lose_r= 0;
void obstacle(void)
{
    int16 i;
    static const int8 WIDTH_THRESHOLD_MIN = 35;//有障碍物赛道的宽度一个范围/
    static const int8 WIDTH_THRESHOLD_MAX = 65;//正常赛道的宽度/ //70

    //static const int8 WIDTH_THRESHOLD_MIN98 = 130;///屏幕最后一行的有障碍的宽度
   // static const int8 WIDTH_THRESHOLD_MAX98 = 158;///屏幕最后一行的正常赛道宽度 即是屏幕内看不到障碍。

//    ob_lose_l=lose_line_test(1, 1, 100, 10);
//    ob_lose_r=lose_line_test(0, 1, 100, 10);
 //  46  42
    width2 = func_abs(r_border[40] - l_border[40]);
    width1 = func_abs(r_border[50] - l_border[50]);

    if((ob_lose_l==-1&&ob_lose_r==-1)&&((width1 > WIDTH_THRESHOLD_MIN && width1< WIDTH_THRESHOLD_MAX && l_effect_flag[50] && r_effect_flag[50])||
       (width2 > WIDTH_THRESHOLD_MIN && width2< WIDTH_THRESHOLD_MAX && l_effect_flag[40] && r_effect_flag[40]))&&
       /*(white_length_max[0][0]<3&&white_length_max[1][0]>70)&&*/!zhongdian_flag&&obstacle_distance==0)
    {

        flag_obstacle = 1;
        for(i=10; i<70; i++)
        {
            temp_ob = func_abs(r_border[i] - r_border[i+3]);
            if(temp_ob>6)  flag_obstacle = 2;//障碍在右边

            temp_ob = func_abs(l_border[i] - l_border[i+3]);
            if(temp_ob>6)  flag_obstacle = 3;//障碍在左边
        }
     }

    for(i=109;i>5;i--) //质变
      {
        if(flag_obstacle == 2) //zuo
        {

            r_border[i]=r_border[i]-70;
            l_border[i] =40;
        }
         if(flag_obstacle == 3)
         {

             l_border[i]=l_border[i]+70;
             r_border[i]=140;
         }
      }

}








